clear all;
servo = [];
torque = [];
speed = [];
Fs = 100; % 采样率（每秒采样点数）
T = 1/Fs; % 采样间隔
% 设计巴特沃斯滤波器
fc = 3; % 截止频率（单位为Hz）
Wn = fc/(Fs/2); % 归一化截止频率
[b, a] = butter(4, Wn, 'low'); % 4阶低通巴特沃斯滤波器

polyfit_n = 5;
%最小二乘拟合阶数

coefficient = zeros(17,polyfit_n+1);
i = 1;

figure(1);
% 绘制伯德图
freqz(b, a);

% 指定 Excel 文件路径和名称
filename = 'aerodynamic lift with speed.xlsx';
sheet1 = 'throttle_0';
% 指定要读取的数据范围，例如：'A1:B10'
dataRange1 = 'A2:C109';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo1 = data(:, 1);
torque1 = data(:, 2);
% 创建采样数据
L = length(data(:, 2)); % 数据长度
t = (0:L-1)*T; % 时间向量
% 计算频谱
Y = fft(torque1); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp1 = P2(1:L/2+1); % 单边频谱
amp1(2:end-1) = 2*amp1(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f1 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter1 = filtfilt(b,a,torque1);
% 计算滤波后频谱
Y = fft(torque_filter1); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf1 = P2(1:L/2+1); % 单边频谱
ampf1(2:end-1) = 2*ampf1(2:end-1); % 去除负频谱并加倍幅值
% 由于存在回程数据，使用最小二乘法拟合 舵片角度-扭矩数据
% 滤波前的拟合
[p,S] = polyfit(servo1,torque1,polyfit_n);
[fit_torque1,fit_delta1] = polyval(p,unique(servo1),S);
% 滤波后的拟合
[p,S] = polyfit(servo1,torque_filter1,polyfit_n);
[fit_torque_filter1,fit_delta_filter1] = polyval(p,unique(servo1),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;

sheet1 = 'throttle_5';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo2 = data(:, 1);
torque2 = data(:, 2);
% 计算频谱
Y = fft(torque2); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp2 = P2(1:L/2+1); % 单边频谱
amp2(2:end-1) = 2*amp2(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f2 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter2 = filtfilt(b,a,torque2);
% 计算滤波后频谱
Y = fft(torque_filter2); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf2 = P2(1:L/2+1); % 单边频谱
ampf2(2:end-1) = 2*ampf2(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo2,torque2,polyfit_n);
[fit_torque2,fit_delta2] = polyval(p,unique(servo2),S);
% 滤波后的拟合
[p,S] = polyfit(servo2,torque_filter2,polyfit_n);
[fit_torque_filter2,fit_delta_filter2] = polyval(p,unique(servo2),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;

sheet1 = 'throttle_10';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo3 = data(:, 1);
torque3 = data(:, 2);
% 计算频谱
Y = fft(torque3); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp3 = P2(1:L/2+1); % 单边频谱
amp3(2:end-1) = 2*amp3(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f3 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter3 = filtfilt(b,a,torque3);
% 计算滤波后频谱
Y = fft(torque_filter3); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf3 = P2(1:L/2+1); % 单边频谱
ampf3(2:end-1) = 2*ampf3(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo3,torque3,polyfit_n);
[fit_torque3,fit_delta3] = polyval(p,unique(servo3),S);
% 滤波后的拟合
[p,S] = polyfit(servo3,torque_filter3,polyfit_n);
[fit_torque_filter3,fit_delta_filter3] = polyval(p,unique(servo3),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;


sheet1 = 'throttle_15';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo4 = data(:, 1);
torque4 = data(:, 2);
% 计算频谱
Y = fft(torque4); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp4 = P2(1:L/2+1); % 单边频谱
amp4(2:end-1) = 2*amp4(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f4 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter4 = filtfilt(b,a,torque4);
% 计算滤波后频谱
Y = fft(torque_filter4); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf4 = P2(1:L/2+1); % 单边频谱
ampf4(2:end-1) = 2*ampf4(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo4,torque4,polyfit_n);
[fit_torque4,fit_delta4] = polyval(p,unique(servo4),S);
% 滤波后的拟合
[p,S] = polyfit(servo4,torque_filter4,polyfit_n);
[fit_torque_filter4,fit_delta_filter4] = polyval(p,unique(servo4),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;


sheet1 = 'throttle_20';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo5 = data(:, 1);
torque5 = data(:, 2);
% 计算频谱
Y = fft(torque5); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp5 = P2(1:L/2+1); % 单边频谱
amp5(2:end-1) = 2*amp5(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f5 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter5 = filtfilt(b,a,torque5);
% 计算滤波后频谱
Y = fft(torque_filter5); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf5 = P2(1:L/2+1); % 单边频谱
ampf5(2:end-1) = 2*ampf5(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo5,torque5,polyfit_n);
[fit_torque5,fit_delta5] = polyval(p,unique(servo5),S);
% 滤波后的拟合
[p,S] = polyfit(servo5,torque_filter5,polyfit_n);
[fit_torque_filter5,fit_delta_filter5] = polyval(p,unique(servo5),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;



sheet1 = 'throttle_25';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo6 = data(:, 1);
torque6 = data(:, 2);
% 计算频谱
Y = fft(torque6); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp6 = P2(1:L/2+1); % 单边频谱
amp6(2:end-1) = 2*amp6(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f6 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter6 = filtfilt(b,a,torque6);
% 计算滤波后频谱
Y = fft(torque_filter6); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf6 = P2(1:L/2+1); % 单边频谱
ampf6(2:end-1) = 2*ampf6(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo6,torque6,polyfit_n);
[fit_torque6,fit_delta6] = polyval(p,unique(servo6),S);
% 滤波后的拟合
[p,S] = polyfit(servo6,torque_filter6,polyfit_n);
[fit_torque_filter6,fit_delta_filter6] = polyval(p,unique(servo6),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;



sheet1 = 'throttle_30';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo7 = data(:, 1);
torque7 = data(:, 2);
% 计算频谱
Y = fft(torque7); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp7 = P2(1:L/2+1); % 单边频谱
amp7(2:end-1) = 2*amp7(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f7 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter7 = filtfilt(b,a,torque7);
% 计算滤波后频谱
Y = fft(torque_filter7); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf7 = P2(1:L/2+1); % 单边频谱
ampf7(2:end-1) = 2*ampf7(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo7,torque7,polyfit_n);
[fit_torque7,fit_delta7] = polyval(p,unique(servo7),S);
% 滤波后的拟合
[p,S] = polyfit(servo7,torque_filter7,polyfit_n);
[fit_torque_filter7,fit_delta_filter7] = polyval(p,unique(servo7),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;



sheet1 = 'throttle_35';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo8 = data(:, 1);
torque8 = data(:, 2);
% 计算频谱
Y = fft(torque8); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp8 = P2(1:L/2+1); % 单边频谱
amp8(2:end-1) = 2*amp8(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f8 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter8 = filtfilt(b,a,torque8);
% 计算滤波后频谱
Y = fft(torque_filter8); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf8 = P2(1:L/2+1); % 单边频谱
ampf8(2:end-1) = 2*ampf8(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo8,torque8,polyfit_n);
[fit_torque8,fit_delta8] = polyval(p,unique(servo8),S);
% 滤波后的拟合
[p,S] = polyfit(servo8,torque_filter8,polyfit_n);
[fit_torque_filter8,fit_delta_filter8] = polyval(p,unique(servo8),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;


sheet1 = 'throttle_40';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo9 = data(:, 1);
torque9 = data(:, 2);
% 计算频谱
Y = fft(torque9); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp9 = P2(1:L/2+1); % 单边频谱
amp9(2:end-1) = 2*amp9(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f9 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter9 = filtfilt(b,a,torque9);
% 计算滤波后频谱
Y = fft(torque_filter9); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf9 = P2(1:L/2+1); % 单边频谱
ampf9(2:end-1) = 2*ampf9(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo9,torque9,polyfit_n);
[fit_torque9,fit_delta9] = polyval(p,unique(servo9),S);
% 滤波后的拟合
[p,S] = polyfit(servo9,torque_filter9,polyfit_n);
[fit_torque_filter9,fit_delta_filter9] = polyval(p,unique(servo9),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;


sheet1 = 'throttle_45';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo10 = data(:, 1);
torque10 = data(:, 2);
% 计算频谱
Y = fft(torque10); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp10 = P2(1:L/2+1); % 单边频谱
amp10(2:end-1) = 2*amp10(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f10 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter10 = filtfilt(b,a,torque10);
% 计算滤波后频谱
Y = fft(torque_filter10); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf10 = P2(1:L/2+1); % 单边频谱
ampf10(2:end-1) = 2*ampf10(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo10,torque10,polyfit_n);
[fit_torque10,fit_delta10] = polyval(p,unique(servo10),S);
% 滤波后的拟合
[p,S] = polyfit(servo10,torque_filter10,polyfit_n);
[fit_torque_filter10,fit_delta_filter10] = polyval(p,unique(servo10),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;

sheet1 = 'throttle_50';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo11 = data(:, 1);
torque11 = data(:, 2);
% 计算频谱
Y = fft(torque11); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp11 = P2(1:L/2+1); % 单边频谱
amp11(2:end-1) = 2*amp11(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f11 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter11 = filtfilt(b,a,torque11);
% 计算滤波后频谱
Y = fft(torque_filter11); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf11 = P2(1:L/2+1); % 单边频谱
ampf11(2:end-1) = 2*ampf11(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo11,torque11,polyfit_n);
[fit_torque11,fit_delta11] = polyval(p,unique(servo11),S);
% 滤波后的拟合
[p,S] = polyfit(servo11,torque_filter11,polyfit_n);
[fit_torque_filter11,fit_delta_filter11] = polyval(p,unique(servo11),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;


sheet1 = 'throttle_55';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo12 = data(:, 1);
torque12 = data(:, 2);
% 计算频谱
Y = fft(torque12); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp12 = P2(1:L/2+1); % 单边频谱
amp12(2:end-1) = 2*amp12(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f12 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter12 = filtfilt(b,a,torque12);
% 计算滤波后频谱
Y = fft(torque_filter12); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf12 = P2(1:L/2+1); % 单边频谱
ampf12(2:end-1) = 2*ampf12(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo12,torque12,polyfit_n);
[fit_torque12,fit_delta12] = polyval(p,unique(servo12),S);
% 滤波后的拟合
[p,S] = polyfit(servo12,torque_filter12,polyfit_n);
[fit_torque_filter12,fit_delta_filter12] = polyval(p,unique(servo12),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;




sheet1 = 'throttle_60';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo13 = data(:, 1);
torque13 = data(:, 2);
% 计算频谱
Y = fft(torque13); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp13 = P2(1:L/2+1); % 单边频谱
amp13(2:end-1) = 2*amp13(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f13 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter13 = filtfilt(b,a,torque13);
% 计算滤波后频谱
Y = fft(torque_filter13); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf13 = P2(1:L/2+1); % 单边频谱
ampf13(2:end-1) = 2*ampf13(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo13,torque13,polyfit_n);
[fit_torque13,fit_delta13] = polyval(p,unique(servo13),S);
% 滤波后的拟合
[p,S] = polyfit(servo13,torque_filter13,polyfit_n);
[fit_torque_filter13,fit_delta_filter13] = polyval(p,unique(servo13),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;


sheet1 = 'throttle_65';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo14 = data(:, 1);
torque14 = data(:, 2);
% 计算频谱
Y = fft(torque14); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp14 = P2(1:L/2+1); % 单边频谱
amp14(2:end-1) = 2*amp14(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f14 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter14 = filtfilt(b,a,torque14);
% 计算滤波后频谱
Y = fft(torque_filter14); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf14 = P2(1:L/2+1); % 单边频谱
ampf14(2:end-1) = 2*ampf14(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo14,torque14,polyfit_n);
[fit_torque14,fit_delta14] = polyval(p,unique(servo14),S);
% 滤波后的拟合
[p,S] = polyfit(servo14,torque_filter14,polyfit_n);
[fit_torque_filter14,fit_delta_filter14] = polyval(p,unique(servo14),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;



sheet1 = 'throttle_70';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo15 = data(:, 1);
torque15 = data(:, 2);
% 计算频谱
Y = fft(torque15); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp15 = P2(1:L/2+1); % 单边频谱
amp15(2:end-1) = 2*amp15(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f15 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter15 = filtfilt(b,a,torque15);
% 计算滤波后频谱
Y = fft(torque_filter15); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf15 = P2(1:L/2+1); % 单边频谱
ampf15(2:end-1) = 2*ampf15(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo15,torque15,polyfit_n);
[fit_torque15,fit_delta15] = polyval(p,unique(servo15),S);
% 滤波后的拟合
[p,S] = polyfit(servo15,torque_filter15,polyfit_n);
[fit_torque_filter15,fit_delta_filter15] = polyval(p,unique(servo15),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;


sheet1 = 'throttle_75';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo16 = data(:, 1);
torque16 = data(:, 2);
% 计算频谱
Y = fft(torque16); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp16 = P2(1:L/2+1); % 单边频谱
amp16(2:end-1) = 2*amp16(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f16 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter16 = filtfilt(b,a,torque16);
% 计算滤波后频谱
Y = fft(torque_filter16); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf16 = P2(1:L/2+1); % 单边频谱
ampf16(2:end-1) = 2*ampf16(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo16,torque16,polyfit_n);
[fit_torque16,fit_delta16] = polyval(p,unique(servo16),S);
% 滤波后的拟合
[p,S] = polyfit(servo16,torque_filter16,polyfit_n);
[fit_torque_filter16,fit_delta_filter16] = polyval(p,unique(servo16),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;



sheet1 = 'throttle_80';
% 使用 xlsread 函数读取数据
[data, headers] = xlsread(filename, sheet1, dataRange1);
% 将数据存储到 MATLAB 的变量中
servo = vertcat(servo,data(:, 1)); % 第一列数据
torque = vertcat(torque,data(:, 2)); % 第一列数据
speed = vertcat(speed,data(:, 3)); % 第一列数据
servo17 = data(:, 1);
torque17 = data(:, 2);
% 计算频谱
Y = fft(torque17); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
amp17 = P2(1:L/2+1); % 单边频谱
amp17(2:end-1) = 2*amp17(2:end-1); % 去除负频谱并加倍幅值
% 创建频率向量
f17 = Fs*(0:(L/2))/L;
%巴特沃斯低通滤波器
torque_filter17 = filtfilt(b,a,torque17);
% 计算滤波后频谱
Y = fft(torque_filter17); % 对信号进行FFT变换
P2 = abs(Y/L); % 双边频谱
ampf17 = P2(1:L/2+1); % 单边频谱
ampf17(2:end-1) = 2*ampf17(2:end-1); % 去除负频谱并加倍幅值
% 滤波前的拟合
[p,S] = polyfit(servo17,torque17,polyfit_n);
[fit_torque17,fit_delta17] = polyval(p,unique(servo17),S);
% 滤波后的拟合
[p,S] = polyfit(servo17,torque_filter17,polyfit_n);
[fit_torque_filter17,fit_delta_filter17] = polyval(p,unique(servo17),S);
% 记录系数
coefficient(i,:) = p;
i = i+1;

figure(2);
subplot(5,2,1);
plot3(speed,torque,servo,'o');
title('舵角/转速-扭矩原始数据曲面');

subplot(5,2,2);
input = [torque,speed];
output = servo;
fit_output = servo_angle_calculation(input);
plot3(speed,torque,fit_output,'o');%servo,speed,torque,'*',
title('舵角/转速-扭矩神经网络拟合数据曲面');

point_size = 3;
subplot(5,2,3);
hold on;
plot(servo1,torque1,'o','MarkerSize',point_size);
plot(servo2,torque2,'o','MarkerSize',point_size);
plot(servo3,torque3,'o','MarkerSize',point_size);
plot(servo4,torque4,'o','MarkerSize',point_size);
plot(servo5,torque5,'o','MarkerSize',point_size);
plot(servo6,torque6,'o','MarkerSize',point_size);
plot(servo7,torque7,'o','MarkerSize',point_size);
plot(servo8,torque8,'o','MarkerSize',point_size);
plot(servo9,torque9,'o','MarkerSize',point_size);
plot(servo10,torque10,'o','MarkerSize',point_size);
plot(servo11,torque11,'o','MarkerSize',point_size);
plot(servo12,torque12,'o','MarkerSize',point_size);
plot(servo13,torque13,'o','MarkerSize',point_size);
plot(servo14,torque14,'o','MarkerSize',point_size);
plot(servo15,torque15,'o','MarkerSize',point_size);
plot(servo16,torque16,'o','MarkerSize',point_size);
plot(servo17,torque17,'o','MarkerSize',point_size);
% legend('thr0','thr5','thr10','thr15','thr20','thr25','thr30','thr35','thr40','thr45','thr50','thr55','thr60','thr65','thr70','thr75','thr80');
colorbar;
title('滤波前扭矩舵片角度散点图');
xlabel('舵片角度/°');
ylabel('扭矩/Nm');
hold off;

subplot(5,2,4);
hold on;
plot(servo1,torque_filter1,'o','MarkerSize',point_size);
plot(servo2,torque_filter2,'o','MarkerSize',point_size);
plot(servo3,torque_filter3,'o','MarkerSize',point_size);
plot(servo4,torque_filter4,'o','MarkerSize',point_size);
plot(servo5,torque_filter5,'o','MarkerSize',point_size);
plot(servo6,torque_filter6,'o','MarkerSize',point_size);
plot(servo7,torque_filter7,'o','MarkerSize',point_size);
plot(servo8,torque_filter8,'o','MarkerSize',point_size);
plot(servo9,torque_filter9,'o','MarkerSize',point_size);
plot(servo10,torque_filter10,'o','MarkerSize',point_size);
plot(servo11,torque_filter11,'o','MarkerSize',point_size);
plot(servo12,torque_filter12,'o','MarkerSize',point_size);
plot(servo13,torque_filter13,'o','MarkerSize',point_size);
plot(servo14,torque_filter14,'o','MarkerSize',point_size);
plot(servo15,torque_filter15,'o','MarkerSize',point_size);
plot(servo16,torque_filter16,'o','MarkerSize',point_size);
plot(servo17,torque_filter17,'o','MarkerSize',point_size);
% legend('thr0','thr5','thr10','thr15','thr20','thr25','thr30','thr35','thr40','thr45','thr50','thr55','thr60','thr65','thr70','thr75','thr80');
colorbar;
title('滤波后扭矩舵片角度散点图');
xlabel('舵片角度/°');
ylabel('扭矩/Nm');
hold off;

subplot(5,2,5);
hold on;
plot(unique(servo1),fit_torque1,unique(servo1),fit_delta1);
plot(unique(servo2),fit_torque2,unique(servo2),fit_delta2);
plot(unique(servo3),fit_torque3,unique(servo3),fit_delta3);
plot(unique(servo4),fit_torque4,unique(servo4),fit_delta4);
plot(unique(servo5),fit_torque5,unique(servo5),fit_delta5);
plot(unique(servo6),fit_torque6,unique(servo6),fit_delta6);
plot(unique(servo7),fit_torque7,unique(servo7),fit_delta7);
plot(unique(servo8),fit_torque8,unique(servo8),fit_delta8);
plot(unique(servo9),fit_torque9,unique(servo9),fit_delta9);
plot(unique(servo10),fit_torque10,unique(servo10),fit_delta10);
plot(unique(servo11),fit_torque11,unique(servo11),fit_delta11);
plot(unique(servo12),fit_torque12,unique(servo12),fit_delta12);
plot(unique(servo13),fit_torque13,unique(servo13),fit_delta13);
plot(unique(servo14),fit_torque14,unique(servo14),fit_delta14);
plot(unique(servo15),fit_torque15,unique(servo15),fit_delta15);
plot(unique(servo16),fit_torque16,unique(servo16),fit_delta16);
plot(unique(servo17),fit_torque17,unique(servo17),fit_delta17);
% legend('thr0','thr5','thr10','thr15','thr20','thr25','thr30','thr35','thr40','thr45','thr50','thr55','thr60','thr65','thr70','thr75','thr80');
colorbar;
title('滤波前扭矩舵片角度拟合曲线图');
xlabel('舵片角度/°');
ylabel('扭矩/Nm');
hold off;

subplot(5,2,6);
hold on;
plot(unique(servo1),fit_torque_filter1,unique(servo1),fit_delta_filter1);
plot(unique(servo2),fit_torque_filter2,unique(servo2),fit_delta_filter2);
plot(unique(servo3),fit_torque_filter3,unique(servo3),fit_delta_filter3);
plot(unique(servo4),fit_torque_filter4,unique(servo4),fit_delta_filter4);
plot(unique(servo5),fit_torque_filter5,unique(servo5),fit_delta_filter5);
plot(unique(servo6),fit_torque_filter6,unique(servo6),fit_delta_filter6);
plot(unique(servo7),fit_torque_filter7,unique(servo7),fit_delta_filter7);
plot(unique(servo8),fit_torque_filter8,unique(servo8),fit_delta_filter8);
plot(unique(servo9),fit_torque_filter9,unique(servo9),fit_delta_filter9);
plot(unique(servo10),fit_torque_filter10,unique(servo10),fit_delta_filter10);
plot(unique(servo11),fit_torque_filter11,unique(servo11),fit_delta_filter11);
plot(unique(servo12),fit_torque_filter12,unique(servo12),fit_delta_filter12);
plot(unique(servo13),fit_torque_filter13,unique(servo13),fit_delta_filter13);
plot(unique(servo14),fit_torque_filter14,unique(servo14),fit_delta_filter14);
plot(unique(servo15),fit_torque_filter15,unique(servo15),fit_delta_filter15);
plot(unique(servo16),fit_torque_filter16,unique(servo16),fit_delta_filter16);
plot(unique(servo17),fit_torque_filter17,unique(servo17),fit_delta_filter17);
% legend('thr0','thr5','thr10','thr15','thr20','thr25','thr30','thr35','thr40','thr45','thr50','thr55','thr60','thr65','thr70','thr75','thr80');
colorbar;
title('滤波后扭矩舵片角度拟合曲线图');
xlabel('舵片角度/°');
ylabel('扭矩/Nm');
hold off;

subplot(5,2,7);
hold on;
plot(t,torque1);
plot(t,torque2);
plot(t,torque3);
plot(t,torque4);
plot(t,torque5);
plot(t,torque6);
plot(t,torque7);
plot(t,torque8);
plot(t,torque9);
plot(t,torque10);
plot(t,torque11);
plot(t,torque12);
plot(t,torque13);
plot(t,torque14);
plot(t,torque15);
plot(t,torque16);
plot(t,torque17);
title('滤波前扭矩时间曲线');
xlabel('时间/t');
ylabel('扭矩/Nm');
% legend('thr0','thr5','thr10','thr15','thr20','thr25','thr30','thr35','thr40','thr45','thr50','thr55','thr60','thr65','thr70','thr75','thr80');
colorbar;
hold off;

subplot(5,2,8);
hold on;
plot(t,torque_filter1);
plot(t,torque_filter2);
plot(t,torque_filter3);
plot(t,torque_filter4);
plot(t,torque_filter5);
plot(t,torque_filter6);
plot(t,torque_filter7);
plot(t,torque_filter8);
plot(t,torque_filter9);
plot(t,torque_filter10);
plot(t,torque_filter11);
plot(t,torque_filter12);
plot(t,torque_filter13);
plot(t,torque_filter14);
plot(t,torque_filter15);
plot(t,torque_filter16);
plot(t,torque_filter17);
% legend('thr0','thr5','thr10','thr15','thr20','thr25','thr30','thr35','thr40','thr45','thr50','thr55','thr60','thr65','thr70','thr75','thr80');
colorbar;
title('滤波后扭矩时间曲线');
xlabel('时间/t');
ylabel('扭矩/Nm');
hold off;


subplot(5,2,9);
hold on;
plot(f1,amp1);
plot(f2,amp2);
plot(f3,amp3);
plot(f4,amp4);
plot(f5,amp5);
plot(f6,amp6);
plot(f7,amp7);
plot(f8,amp8);
plot(f9,amp9);
plot(f10,amp10);
plot(f11,amp11);
plot(f12,amp12);
plot(f13,amp13);
plot(f14,amp14);
plot(f15,amp15);
plot(f16,amp16);
plot(f17,amp17);
colorbar;
title('滤波前扭矩数据频谱图');
xlabel('频率/hz');
ylabel('幅值/dB');
hold off;

subplot(5,2,10);
hold on;
plot(f1,ampf1);
plot(f2,ampf2);
plot(f3,ampf3);
plot(f4,ampf4);
plot(f5,ampf5);
plot(f6,ampf6);
plot(f7,ampf7);
plot(f8,ampf8);
plot(f9,ampf9);
plot(f10,ampf10);
plot(f11,ampf11);
plot(f12,ampf12);
plot(f13,ampf13);
plot(f14,ampf14);
plot(f15,ampf15);
plot(f16,ampf16);
plot(f17,ampf17);
title('滤波后扭矩数据频谱图');
xlabel('频率/hz');
ylabel('幅值/dB');
colorbar;
hold off;

leg = string({'y=','y=','y=','y=','y=','y=','y=','y=','y=','y=','y=','y=','y=','y=','y=','y=','y='}); %#ok<STRCLQT>
for i = 1:17
    for j = 1:polyfit_n+1
        leg(i) = leg(i) + string(coefficient(i,j)) + 'x^'+ string(polyfit_n-j+1);
        if j < polyfit_n+1
            leg(i) = leg(i) + '+';
        end
    end
end

figure(3);
hold on;
plot(unique(servo1),fit_torque_filter1,unique(servo1),fit_delta_filter1);
plot(unique(servo2),fit_torque_filter2,unique(servo2),fit_delta_filter2);
plot(unique(servo3),fit_torque_filter3,unique(servo3),fit_delta_filter3);
plot(unique(servo4),fit_torque_filter4,unique(servo4),fit_delta_filter4);
plot(unique(servo5),fit_torque_filter5,unique(servo5),fit_delta_filter5);
plot(unique(servo6),fit_torque_filter6,unique(servo6),fit_delta_filter6);
plot(unique(servo7),fit_torque_filter7,unique(servo7),fit_delta_filter7);
plot(unique(servo8),fit_torque_filter8,unique(servo8),fit_delta_filter8);
plot(unique(servo9),fit_torque_filter9,unique(servo9),fit_delta_filter9);
plot(unique(servo10),fit_torque_filter10,unique(servo10),fit_delta_filter10);
plot(unique(servo11),fit_torque_filter11,unique(servo11),fit_delta_filter11);
plot(unique(servo12),fit_torque_filter12,unique(servo12),fit_delta_filter12);
plot(unique(servo13),fit_torque_filter13,unique(servo13),fit_delta_filter13);
plot(unique(servo14),fit_torque_filter14,unique(servo14),fit_delta_filter14);
plot(unique(servo15),fit_torque_filter15,unique(servo15),fit_delta_filter15);
plot(unique(servo16),fit_torque_filter16,unique(servo16),fit_delta_filter16);
plot(unique(servo17),fit_torque_filter17,unique(servo17),fit_delta_filter17);
legend(leg(1),leg(2),leg(3),leg(4),leg(5),leg(6),leg(7),leg(8),leg(9),leg(10),leg(11),leg(12),leg(13),leg(14),leg(15),leg(16),leg(17));
colorbar;
title('滤波后扭矩舵片角度拟合曲线图');
xlabel('舵片角度/°');
ylabel('扭矩/Nm');
hold off;


figure(4);
plot(torque,servo,'o');
title('转速扭矩-舵角数据散点图');
